// Working on Ln46

#include "ddmcmc_meanshift.h"

using namespace cv;
using namespace std;


// ==========  Constants  ==========
static const int p_max = 3;	// max space dim;  MAX(p_max) == 6 !!

static const double	BIG_NUM = 1.0e+20;
static const int  MAX_TRIAL = 10; // Few more trials at the end
static const int MAX_RECTS = 50;
static const int RAND_SEED = 29254088;

static int act_threshold;

DDMCMC_MeanShiftClustering::DDMCMC_MeanShiftClustering(Mat mOriginalImg){
	mImg = mOriginalImg.clone();
	rows = mImg.rows;
	cols = mImg.cols;
	pxCount = rows*cols;

	_rg_colms = 0;
}

DDMCMC_MeanShiftClustering::~DDMCMC_MeanShiftClustering(){

}

void DDMCMC_MeanShiftClustering::init_neigh()
{
	my_neigh[0] = -cols - 1;  my_neigh[1] = -cols;
	my_neigh[2] = -cols + 1;  my_neigh[3] = +1;
	my_neigh[4] = +cols + 1;  my_neigh[5] = +cols;
	my_neigh[6] = +cols - 1;  my_neigh[7] = -1;

	my_neigh_r[0] = -_rg_colms - 1;  my_neigh_r[1] = -_rg_colms;
	my_neigh_r[2] = -_rg_colms + 1;  my_neigh_r[3] = +1;
	my_neigh_r[4] = +_rg_colms + 1;  my_neigh_r[5] = +_rg_colms;
	my_neigh_r[6] = +_rg_colms - 1;  my_neigh_r[7] = -1;
}

// Init matrices parameters
void DDMCMC_MeanShiftClustering::init_matr()
{
	// General statistic parameters for X.
	double	Mg[p_max];         //sample mean of X
	double	C[p_max][p_max];   //sample covariance matrix of X
	covariance_w(count, _n_colors, _k, _col_all, _m_colors, Mg, C);

	// Adaptation
	double my_th = C[0][0] + C[1][1] + C[2][2];
	int active_gen_contor = 1;

	if (auto_segm)
		fix_RADIUS[0] = gen_RADIUS[option] * sqrt(my_th / 100);
	else
	{
		active_gen_contor = rect_gen_contor;
		for (int i = 0; i<active_gen_contor; i++)
			fix_RADIUS[i] = rect_RADIUS[i] * sqrt(my_th / 100);
	}
	final_RADIUS = fix_RADIUS[active_gen_contor - 1] * 1.26;
	max_dist = fix_RADIUS[0] * fix_RADIUS[0];
#ifdef TRACE
	printf("\n %.2f %.2f ", fix_RADIUS[0], final_RADIUS);
#endif
	act_threshold = (int)((my_threshold[option]>sqrt((float)count) / my_rap[option]) ?
		my_threshold[option] : sqrt((float)count) / my_rap[option]);
	cerr << ":";
}

void DDMCMC_MeanShiftClustering::meanShiftInitialization(int &active_gen_contor){
	// neighborhood initialization
	init_neigh();

	_labels = new uchar[pxCount];
	memset(_labels, 255, pxCount);

	
	_remn_cnt = pxCount;
	_n_col_remain = _n_colors;

	_remn_cnt_map = new int[pxCount];
	_col_remain = new int[_n_colors];
	_m_col_remain = new int[_n_colors];

	for (int i = 0; i< pxCount; i++)
		_remn_cnt_map[i] = i;
	for (int i = 0; i< _n_colors; i++)
		_col_remain[i] = i;
	memcpy(_m_col_remain, _m_colors, sizeof(int)*_n_colors);

	active_gen_contor = 1;
	
	init_matr();
	delete[] _m_colors;
	_m_colors = NULL;
}


/* Corresponding to the original funciton "ms_segment"
 * The argument long selects, unsigned int seed_default, bool block, are omiited.
 */
void DDMCMC_MeanShiftClustering::meanShiftClustering(Rect rRects[], int n_rects, int nOption){
	int		contor_trials = 0, active_gen_contor;
	double	T[MAX_RECTS][3];
	int		TI[MAX_RECTS][3];
	double	fL, fU, fV, RAD2, q;
	int		i, k;

	_k = 3;
	srand(RAND_SEED);

	// !!!!!!!!!!!!!! Check the value of !!n_rects!! and active_gen_contor after initialization
	meanShiftInitialization(active_gen_contor);

	// Mean shift algorithm and removal of the detected feature
	int rect;
	for (rect = 0; rect < n_rects; rect++)
	{
		_rg_cnt = rRects[rect].width * rRects[rect].height;
		cut_rectangle(&rRects[rect]);

	MS:
		if (contor_trials){
			_max_sub_samp = 1;
		}

		q = my_sampling(rect, T);
		if (q == -BIG_NUM || q == BIG_NUM){
			if (contor_trials++ < MAX_TRIAL){
				goto MS;
			}else{
				break;
			}
		}

		double final_T[p_max];
		for (i = 0; i < _k; i++) 
			final_T[i] = T[rect][i];

		int *selected = new int[pxCount];
		uchar *sel_col = new uchar[_n_colors];
		uchar *my_class = new uchar[pxCount];

		int my_contor = 0, color_cnt = -1, how_many = 10;
		while (color_cnt++ < active_gen_contor - 1)
		{
			set_RADIUS(color_cnt, 0);
			int gen_contor = 0;
			bool last_loop = 0;

			while (gen_contor++<how_many)
			{
				memset(sel_col, 0, _n_colors);
				// execuation of mean shift algorightm
				new_auto_loop(final_T, sel_col);
				fL = T[rect][0] - final_T[0];
				fU = T[rect][1] - final_T[1];
				fV = T[rect][2] - final_T[2];
				RAD2 = fL * fL + fU * fU + fV * fV;
				if (RAD2<0.1)
				{
					my_contor = 0;
					memset(my_class, 0, pxCount);
					for (k = 0; k < _remn_cnt; k++)
					{
						int p = _remn_cnt_map[k];
						if (sel_col[_col_index[p]])
						{
							selected[my_contor++] = p;
							my_class[p] = 1;
						}
					}
					break;
				}
				else
				{
					T[rect][0] = final_T[0];
					T[rect][1] = final_T[1];
					T[rect][2] = final_T[2];
				}
			}
		}

		if (nOption == 0) test_neigh(my_class, selected, &my_contor, 2);
		else if (nOption == 1) test_neigh(my_class, selected, &my_contor, 1);
		else if (nOption == 2) test_neigh(my_class, selected, &my_contor, 0);

		if (my_contor < act_threshold){
			delete[] my_class;
			delete[] sel_col;
			delete[] selected;
			if (contor_trials++ < MAX_TRIAL) goto MS;
			else                          break;
		}

		my_actual(my_class);

		convert_LUV_RGB(final_T, TI[rect]);
		for (k = 0; k < pxCount; k++)
		if (my_class[k])
			_labels[k] = rect;
		delete[] my_class; delete[] sel_col; delete[] selected;
	}
	n_rects = rect;
	uchar** isegment = new uchar*[3];
	int j;
	for (j = 0; j < 3; j++)
	{
		isegment[j] = new uchar[pxCount];
		memset(isegment[j], 0, pxCount);
	}
	uchar *isegment0 = isegment[0];
	uchar *isegment1 = isegment[1];
	uchar *isegment2 = isegment[2];

	uchar *my_class = new uchar[pxCount];
	for (k = 0; k < pxCount; k++)
	if (_labels[k] == 255) _labels[k] = n_rects;

	uchar inv_map[MAX_RECTS];
	for (k = 0; k < n_rects; k++) inv_map[k] = k;

	if (nOption == 0)  conn_comp(my_class, &n_rects, inv_map, T, 10, 0);
	else if (nOption == 1) conn_comp(my_class, &n_rects, inv_map, T, 20, 0);
	else conn_comp(my_class, &n_rects, inv_map, T, act_threshold, 0);
	optimize(T, n_rects);
	for (k = 0; k < n_rects; k++) convert_LUV_RGB(T[k], TI[k]);

	// Postprocessing
	if (nOption == 1 || option == 2)
	for (k = 2; k < 10; k += 2) conn_comp(my_class, &n_rects, inv_map, T, k, 1);

	uchar my_max_ind;
	ImageLabels.SetDimension(rows, cols);
	for (k = 0; k<pxCount; k++)
	{
		if ((my_max_ind = _labels[k]) != n_rects)
		{
			int *ti = TI[my_max_ind];
			isegment0[k] = ti[0]; isegment1[k] = ti[1]; isegment2[k] = ti[2];
			ImageLabels(k) = my_max_ind;
		}
	}

	delete[] my_class;

	delete[] _m_col_remain;
	delete[] _col_remain;
	delete[] _remn_cnt_map;

	delete[] _labels;		_labels = NULL;
	delete[] _col_index;	_col_index = NULL;
	delete[] _col0;		_col0 = NULL;
	delete[] _col1;		_col1 = NULL;
	delete[] _col2;		_col2 = NULL;
	delete[] _col_all;		_col_all = NULL;

	//copy result to image
	for (long mm = 0; mm < pxCount; mm++)
	{
		R(mm) = isegment[0][mm];
		G(mm) = isegment[1][mm];
		B(mm) = isegment[2][mm];
	}

	cerr << endl << "Original Colors: " << _n_colors << endl;
	cerr << "Segment. Colors: " << n_rects << endl;

	return;
}

// Corresponding to the original function "SegmentationMeanshift".
void DDMCMC_MeanShiftClustering::meanShiftSegmentation(int nOption){
	cvtColor(mImg, mImg, CV_BGR2Luv);

	//long lSelects = Lightness | Ustar | Vstar;
	int nRectCount = MAX_RECTS;
	Rect rRects[MAX_RECTS];
	for (int i = 0; i < MAX_RECTS; i++){
		rRects[i] = Rect(0, 0, mImg.rows, mImg.cols);
	}

	meanShiftClustering(rRects, nRectCount, nOption);

	
	ms_clustering = pSeg->GetLabels();

	Raster<int>	image;
	image = ms_clustering;
	CopyPseudoColor(image);

	CopyFromRasterColor(*m_pDib, *((Raster_Color<uchar>*)pSeg));
	pSeg->CreateLUVspace();
	
}



int main(){
	Mat mImg;
	mImg = imread("D:/Works/Research/DDMCMC/Test/test.bmp");

	DDMCMC_MeanShiftClustering msClustering(mImg);
	msClustering.meanShiftSegmentation(1);

	return 0;
}